use core::slice;

use cmsis_rtos2_rs::osWaitForever;

#[cfg(feature = "base_app")]
use cmsis_rtos2_rs::static_mem::mq_size;
use cmsis_rtos2_rs::static_mem::EventFlags;
#[cfg(feature = "base_app")]
use cmsis_rtos2_rs::static_mem::MessageQueue;

use periph_utils::stm32f1;
use periph_utils::stm32f1::stm32f103::NVIC;
use periph_utils::stm32f1::stm32f103::{interrupt, Interrupt};
use periph_utils::CanConfig;
use periph_utils::GpioConfig;
use periph_utils::PeriphReg;
use proto_bytes::proto2::ToProtoBytes2;
use utils::ByteBuffer;

#[allow(unused_imports)]
use crate::address::{IPC, MY};

#[derive(CanConfig, GpioConfig)]
#[can(init = "can_init", baud_rate = 1000000, fb_count = 1)]
#[gpio(
    init = "gpio_init",
    io(name = "tx", io = "PA12", mode = "AF_PP"),
    io(name = "rx", io = "PA11", mode = "IN_UP"),
    io(name = "s", io = "PA15", mode = "IN_FLOAT")
)]
struct Init;

#[derive(PeriphReg)]
#[periph(periph = "CAN1")]
struct CAN;

/**
*      const MASK: u32 = (0xFF00 << 3) | 0x04;
*      const VALUE: u32 = (((MY as u32) << 8) << 3) | 0x04;
       CAN.fb[0].fr1.write(|w| w.fb().variant(VALUE));
       CAN.fb[0].fr2.write(|w| w.fb().variant(MASK));
*/

#[cfg(feature = "cp_drop")]
const DST: u8 = IPC;

#[cfg(not(feature = "cp_drop"))]
const DST: u8 = MY;

impl Init {
    const FB0_MASK: u32 = (0xFF00 << 3) | 0x04;
    const FB0_VALUE: u32 = (((DST as u32) << 8) << 3) | 0x04;
}

const FLAG_TX: u32 = 0x01;
const FLAG_PKG: u32 = 0x02;

#[cfg(feature = "base_app")]
const MSG_COUNT: usize = 32;

struct Context {
    flags: EventFlags,
    #[cfg(feature = "base_app")]
    mq: MessageQueue<Frame, MSG_COUNT, { mq_size::<Frame>(MSG_COUNT) }>,
    write_buf: ByteBuffer<255>,
}

impl Context {
    const fn uninit() -> Self {
        Self {
            flags: EventFlags::uninit(),

            #[cfg(feature = "base_app")]
            mq: MessageQueue::uninit(),

            write_buf: ByteBuffer::const_new(),
        }
    }

    unsafe fn init_once(&self) {
        self.flags.init_once("cmd_can");

        #[cfg(feature = "base_app")]
        self.mq.init_once("cmd_can");

        self.flags.set_flags(FLAG_PKG | FLAG_TX);
    }
}

static CTX: Context = Context::uninit();

pub unsafe fn init() {
    Init::gpio_init();
    Init::can_init();
    
    CTX.init_once();
    NVIC::unmask(Interrupt::USB_HP_CAN_TX);

    #[cfg(feature = "base_app")]
    NVIC::unmask(Interrupt::USB_LP_CAN_RX0);
}

fn write_pkg(dst: u8, src: u8, buf: &[u8]) {
    let mut index = 0;
    let mut count = 0;

    while index < buf.len() {
        let mut n = buf.len() - index;
        if n > 8 {
            n = 8;
        }
        let flag = (index + n) >= buf.len();

        let id = count | ((dst as u32) << 8) | ((src as u32) << 16) | ((flag as u32) << 24);

        _ = CTX.flags.wait_any_flags(FLAG_TX, osWaitForever);
        Init::can_write_ext_frame(id, buf[index..].as_ptr(), n as u8);
        index += n;
        count += 1;
    }
}

pub fn write<T: ToProtoBytes2>(x: T) {
    _ = CTX.flags.wait_any_flags(FLAG_PKG, osWaitForever);

    unsafe {
        let ptr = CTX.write_buf.as_mut_ptr();
        let len = x.to_bytes(ptr);
        let src = *ptr.add(5);
        let dst = *ptr.add(6);
        write_pkg(dst, src, slice::from_raw_parts(ptr, len));
    }

    _ = CTX.flags.set_flags(FLAG_PKG);
}

#[cfg(feature = "base_app")]
pub fn take() -> Frame {
    CTX.mq.take()
}

#[interrupt]
unsafe fn USB_HP_CAN_TX() {
    if CAN.tsr.read().rqcp0().bit_is_set() {
        CAN.tsr.write(|w| w.rqcp0().set_bit());
        CTX.flags.set_flags(FLAG_TX);
    }
}

#[cfg(feature = "base_app")]
#[interrupt]
unsafe fn USB_LP_CAN_RX0() {
    while CAN.rfr[0].read().fmp().bits() != 0 {
        let (id, len, data) = Init::can_read();
        let frame = Frame { id, len, data };
        _ = CTX.mq.post(&frame);
    }
}

#[cfg(feature = "base_app")]
#[derive(Clone, Copy)]
pub struct Frame {
    id: u32,
    len: u8,
    data: [u8; 8],
}

#[cfg(feature = "base_app")]
impl Frame {
    pub fn data<'a>(&'a self) -> &'a [u8] {
        &self.data[..(self.len as usize)]
    }

    pub fn pkg_seq(&self) -> u8 {
        self.id as u8
    }

    pub fn dst(&self) -> u8 {
        (self.id >> 8) as u8
    }

    pub fn src(&self) -> u8 {
        (self.id >> 16) as u8
    }

    pub fn is_complete(&self) -> bool {
        (self.id & (0x01 << 24)) != 0x00
    }
}
